function [ C0 ] = dcmnormortho_direct( CHat ) %#codegen
%DCMNORMORTHO_DIRECT 对方向余弦矩阵进行规范正交化，按精确的最优正交化方法-直接法
%
% Input Arguments:
% # CHat: 3*3矩阵，方向余弦矩阵
%
% Output Arguments:
% # C0: 3*3矩阵，正交化后的方向余弦矩阵

A = CHat*CHat';
[Q, T] = eig(A); % 求A的特征值和特征向量，代替sqrtm中的schur分解
diagT = diag(T); % A的特征值
sqT = sqrt(diagT); % A的特征值开方
X = bsxfun(@times, Q, sqT.')*Q';
X = (X+X')/2;
C0 = real(X) \ CHat;
end